using System;

public class ECEF
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }

    public ECEF(double x, double y, double z)
    {
        X = x;
        Y = y;
        Z = z;
    }

    public override string ToString()
    {
        return $"ECEF(X: {X:F6}, Y: {Y:F6}, Z: {Z:F6})";
    }
}

public class LLA
{
    public double Latitude { get; set; }  // 纬度 (度)
    public double Longitude { get; set; } // 经度 (度)
    public double Altitude { get; set; }  // 海拔高度 (米)

    public LLA(double latitude, double longitude, double altitude)
    {
        Latitude = latitude;
        Longitude = longitude;
        Altitude = altitude;
    }

    public override string ToString()
    {
        return $"LLA(Lat: {Latitude:F9}°, Lon: {Longitude:F9}°, Alt: {Altitude:F6}m)";
    }
}

public static class HighPrecisionCoordinateConverter
{
    // 高精度WGS84椭球体参数
    private const double A = 6378137.0;                    // 长半轴 (米)
    private const double F = 1.0 / 298.257223563;          // 扁率
    private const double B = A * (1.0 - F);                // 短半轴 (米)
    private const double E2 = 2.0 * F - F * F;             // 第一偏心率的平方
    private const double E4 = E2 * E2;
    private const double E6 = E4 * E2;
    private const double E8 = E4 * E4;
    private const double E_P2 = E2 / (1.0 - E2);           // 第二偏心率的平方

    // 用于迭代计算的精度阈值
    private const double EPSILON = 1e-15;
    private const int MAX_ITERATIONS = 20;

    /// <summary>
    /// 高精度ECEF转LLA转换 (使用Bowring方法改进)
    /// </summary>
    public static LLA ECEFToLLA_HighPrecision(ECEF ecef)
    {
        double x = ecef.X;
        double y = ecef.Y;
        double z = ecef.Z;

        // 1. 计算经度 (直接计算，精度很高)
        double lon = Math.Atan2(y, x);

        // 2. 使用Bowring方法计算纬度 (提高精度)
        double p = Math.Sqrt(x * x + y * y);

        // 初始估计
        double theta = Math.Atan2(z * A, p * B);
        double sinTheta = Math.Sin(theta);
        double cosTheta = Math.Cos(theta);

        // 迭代改进纬度计算
        double lat = Math.Atan2(z + E_P2 * B * sinTheta * sinTheta * sinTheta,
                               p - E2 * A * cosTheta * cosTheta * cosTheta);

        // 3. 使用多次迭代进一步提高精度
        for (int i = 0; i < MAX_ITERATIONS; i++)
        {
            double sinLat = Math.Sin(lat);
            double cosLat = Math.Cos(lat);

            double N = A / Math.Sqrt(1.0 - E2 * sinLat * sinLat);
            double alt = p / cosLat - N;

            double newLat = Math.Atan2(z, p * (1.0 - E2 * N / (N + alt)));

            // 检查收敛
            if (Math.Abs(newLat - lat) < EPSILON)
            {
                lat = newLat;
                break;
            }
            lat = newLat;
        }

        // 4. 最终计算高度
        double finalSinLat = Math.Sin(lat);
        double finalCosLat = Math.Cos(lat);

        double N_final = A / Math.Sqrt(1.0 - E2 * finalSinLat * finalSinLat);
        double alt_final = p / finalCosLat - N_final;

        // 5. 处理极点特殊情况
        if (finalCosLat < 1e-12)
        {
            alt_final = Math.Abs(z) - B;
        }

        // 转换为度
        lat = lat * 180.0 / Math.PI;
        lon = lon * 180.0 / Math.PI;

        return new LLA(lat, lon, alt_final);
    }

    /// <summary>
    /// 使用Ferrari方法的高精度转换 (替代算法验证)
    /// </summary>
    public static LLA ECEFToLLA_Ferrari(ECEF ecef)
    {
        double x = ecef.X;
        double y = ecef.Y;
        double z = ecef.Z;

        double lon = Math.Atan2(y, x);
        double p = Math.Sqrt(x * x + y * y);

        // Ferrari方法的迭代计算
        double lat = Math.Atan2(z, p);
        double alt = 0.0;

        for (int i = 0; i < MAX_ITERATIONS; i++)
        {
            double sinLat = Math.Sin(lat);
            double N = A / Math.Sqrt(1.0 - E2 * sinLat * sinLat);
            alt = p / Math.Cos(lat) - N;

            double newLat = Math.Atan2(z, p * (1.0 - E2 * N / (N + alt)));

            if (Math.Abs(newLat - lat) < EPSILON)
            {
                lat = newLat;
                break;
            }
            lat = newLat;
        }

        return new LLA(lat * 180.0 / Math.PI, lon * 180.0 / Math.PI, alt);
    }

    /// <summary>
    /// 高精度LLA转ECEF转换
    /// </summary>
    public static ECEF LLAToECEF_HighPrecision(LLA lla)
    {
        // 使用高精度三角函数
        double lat = lla.Latitude * Math.PI / 180.0;
        double lon = lla.Longitude * Math.PI / 180.0;
        double alt = lla.Altitude;

        double sinLat = Math.Sin(lat);
        double cosLat = Math.Cos(lat);
        double sinLon = Math.Sin(lon);
        double cosLon = Math.Cos(lon);

        // 计算卯酉圈曲率半径 (使用扩展精度)
        double sin2Lat = sinLat * sinLat;
        double denom = Math.Sqrt(1.0 - E2 * sin2Lat);
        double N = A / denom;

        double x = (N + alt) * cosLat * cosLon;
        double y = (N + alt) * cosLat * sinLon;
        double z = (N * (1.0 - E2) + alt) * sinLat;

        return new ECEF(x, y, z);
    }

    /// <summary>
    /// 使用Vincenty方法的极高精度转换 (毫米级精度)
    /// </summary>
    public static LLA ECEFToLLA_Vincenty(ECEF ecef)
    {
        double x = ecef.X;
        double y = ecef.Y;
        double z = ecef.Z;

        double lon = Math.Atan2(y, x);
        double p = Math.Sqrt(x * x + y * y);

        // Vincenty迭代
        double lat = Math.Atan2(z, p * (1.0 - E2));
        double alt = 0.0;

        for (int i = 0; i < MAX_ITERATIONS; i++)
        {
            double sinLat = Math.Sin(lat);
            double N = A / Math.Sqrt(1.0 - E2 * sinLat * sinLat);
            alt = p / Math.Cos(lat) - N;

            double denominator = 1.0 - E2 * N / (N + alt);
            double newLat = Math.Atan2(z, p * denominator);

            if (Math.Abs(newLat - lat) < EPSILON * 0.1) // 更严格的收敛条件
            {
                lat = newLat;
                break;
            }
            lat = newLat;
        }

        return new LLA(lat * 180.0 / Math.PI, lon * 180.0 / Math.PI, alt);
    }
}

class Program
{
    static void Main()
    {
        Console.WriteLine("高精度ECEF转LLA坐标转换测试");
        Console.WriteLine("==========================\n");

        // 高精度测试用例
        var testCases = new[]
        {
            new {
                Name = "测试用例1: 格林尼治天文台",
                ECEF = new ECEF(3980601.507, -0.001, 4966863.247),
                ExpectedLLA = new LLA(51.4779, 0.0, 45.0)
            },
            new {
                Name = "测试用例2: 北极点",
                ECEF = new ECEF(0.000001, 0.000001, 6356752.314245),
                ExpectedLLA = new LLA(90.0, 0.0, 0.0)
            },
            new {
                Name = "测试用例3: 赤道精确点",
                ECEF = new ECEF(6378137.000000, 0.000000, 0.000000),
                ExpectedLLA = new LLA(0.0, 0.0, 0.0)
            },
            new {
                Name = "测试用例4: 北京高精度",
                ECEF = new ECEF(-2148344.0, 4426641.0, 4044655.0),
                ExpectedLLA = new LLA(39.9087, 116.3975, 44.0)
            }
        };

        foreach (var test in testCases)
        {
            TestHighPrecisionConversion(test.Name, test.ECEF, test.ExpectedLLA);
        }

        // 精度对比测试
        Console.WriteLine("精度对比测试");
        Console.WriteLine("============\n");

        var precisionTest = new ECEF(3980601.123456, 123456.789012, 4966863.123456);
        ComparePrecisionMethods(precisionTest);
    }

    static void TestHighPrecisionConversion(string testName, ECEF ecef, LLA expectedLLA)
    {
        Console.WriteLine($"{testName}");
        Console.WriteLine($"输入ECEF: {ecef}");
        Console.WriteLine($"预期LLA: {expectedLLA}");

        // 三种高精度方法
        LLA result1 = HighPrecisionCoordinateConverter.ECEFToLLA_HighPrecision(ecef);
        LLA result2 = HighPrecisionCoordinateConverter.ECEFToLLA_Ferrari(ecef);
        LLA result3 = HighPrecisionCoordinateConverter.ECEFToLLA_Vincenty(ecef);

        Console.WriteLine($"Bowring方法: {result1}");
        Console.WriteLine($"Ferrari方法: {result2}");
        Console.WriteLine($"Vincenty方法: {result3}");

        // 验证反向转换
        ECEF reverse1 = HighPrecisionCoordinateConverter.LLAToECEF_HighPrecision(result1);
        Console.WriteLine($"反向ECEF: {reverse1}");

        double forwardError = CalculateLLAError(expectedLLA, result1);
        double reverseError = CalculateECEFError(ecef, reverse1);

        Console.WriteLine($"正向误差: {forwardError * 111000:F6} 米");
        Console.WriteLine($"反向误差: {reverseError:F6} 米");
        Console.WriteLine($"方法间最大差异: {CalculateMaxDifference(result1, result2, result3):F6} 米");
        Console.WriteLine();
    }

    static void ComparePrecisionMethods(ECEF ecef)
    {
        Console.WriteLine($"精度对比 - 输入: {ecef}");

        var results = new[]
        {
            HighPrecisionCoordinateConverter.ECEFToLLA_HighPrecision(ecef),
            HighPrecisionCoordinateConverter.ECEFToLLA_Ferrari(ecef),
            HighPrecisionCoordinateConverter.ECEFToLLA_Vincenty(ecef)
        };

        // 计算统计信息
        double avgLat = 0, avgLon = 0, avgAlt = 0;
        foreach (var result in results)
        {
            avgLat += result.Latitude;
            avgLon += result.Longitude;
            avgAlt += result.Altitude;
        }
        avgLat /= results.Length;
        avgLon /= results.Length;
        avgAlt /= results.Length;

        Console.WriteLine($"平均值 - Lat: {avgLat:F12}°, Lon: {avgLon:F12}°, Alt: {avgAlt:F9}m");

        // 计算标准差
        double stdLat = 0, stdLon = 0, stdAlt = 0;
        foreach (var result in results)
        {
            stdLat += Math.Pow(result.Latitude - avgLat, 2);
            stdLon += Math.Pow(result.Longitude - avgLon, 2);
            stdAlt += Math.Pow(result.Altitude - avgAlt, 2);
        }
        stdLat = Math.Sqrt(stdLat / results.Length);
        stdLon = Math.Sqrt(stdLon / results.Length);
        stdAlt = Math.Sqrt(stdAlt / results.Length);

        Console.WriteLine($"标准差 - Lat: {stdLat * 111000:E6} m, Lon: {stdLon * 111000:E6} m, Alt: {stdAlt:E6} m");
        Console.WriteLine();
    }

    static double CalculateLLAError(LLA expected, LLA calculated)
    {
        double latError = Math.Abs(expected.Latitude - calculated.Latitude);
        double lonError = Math.Abs(expected.Longitude - calculated.Longitude);
        return Math.Sqrt(latError * latError + lonError * lonError);
    }

    static double CalculateECEFError(ECEF expected, ECEF calculated)
    {
        double dx = expected.X - calculated.X;
        double dy = expected.Y - calculated.Y;
        double dz = expected.Z - calculated.Z;
        return Math.Sqrt(dx * dx + dy * dy + dz * dz);
    }

    static double CalculateMaxDifference(LLA result1, LLA result2, LLA result3)
    {
        double maxDiff = 0;
        var results = new[] { result1, result2, result3 };

        for (int i = 0; i < results.Length; i++)
        {
            for (int j = i + 1; j < results.Length; j++)
            {
                double latDiff = Math.Abs(results[i].Latitude - results[j].Latitude) * 111000;
                double lonDiff = Math.Abs(results[i].Longitude - results[j].Longitude) * 111000;
                double altDiff = Math.Abs(results[i].Altitude - results[j].Altitude);

                maxDiff = Math.Max(maxDiff, Math.Max(latDiff, Math.Max(lonDiff, altDiff)));
            }
        }

        return maxDiff;
    }
}